#pragma once

#include "opencv2/opencv.hpp"
#include <string>
#include <Windows.h>
#include "slStruct.h"
#include "fstream"
#include "sstream"
#include "io.h"
#ifndef slLog
#define slLog Log::getInstance()
#endif

using namespace cv;

namespace SL
{
    void writeIntrinsic(std::string filename, Intrinsic &intrinsic)
    {
        std::ofstream output(filename);
        output << intrinsic.matrix.at<float>(0, 0) << " " << intrinsic.matrix.at<float>(0, 1) << " " << intrinsic.matrix.at<float>(0, 2) << std::endl;
        output << intrinsic.matrix.at<float>(1, 0) << " " << intrinsic.matrix.at<float>(1, 1) << " " << intrinsic.matrix.at<float>(1, 2) << std::endl;
        output << intrinsic.matrix.at<float>(2, 0) << " " << intrinsic.matrix.at<float>(2, 1) << " " << intrinsic.matrix.at<float>(2, 2) << std::endl;
        for (int i = 0; i < intrinsic.kc.size(); i++)
        {
            output << intrinsic.kc[i] << " ";
        }
        output << std::endl;
        output.close();
    }

    void writeExtrinsic(std::string filename, Extrinsic extrinsic)
    {
        std::ofstream output(filename);
        Matx44d mat = extrinsic.matrix;
        output << mat(0, 0) << " " << mat(0, 1) << " " << mat(0, 2) << " " << mat(0, 3) << std::endl;
        output << mat(1, 0) << " " << mat(1, 1) << " " << mat(1, 2) << " " << mat(1, 3) << std::endl;
        output << mat(2, 0) << " " << mat(2, 1) << " " << mat(2, 2) << " " << mat(2, 3) << std::endl;
        output << mat(3, 0) << " " << mat(3, 1) << " " << mat(3, 2) << " " << mat(3, 3) << std::endl;
        output << std::endl;

        Matx33d fundamental = extrinsic.fundamental;
        output << fundamental(0, 0) << " " << fundamental(0, 1) << " " << fundamental(0, 2) << std::endl;
        output << fundamental(1, 0) << " " << fundamental(1, 1) << " " << fundamental(1, 2) << std::endl;
        output << fundamental(2, 0) << " " << fundamental(2, 1) << " " << fundamental(2, 2) << std::endl;
        output << std::endl;

        Matx33d essential = extrinsic.essential;
        output << essential(0, 0) << " " << essential(0, 1) << " " << essential(0, 2) << std::endl;
        output << essential(1, 0) << " " << essential(1, 1) << " " << essential(1, 2) << std::endl;
        output << essential(2, 0) << " " << essential(2, 1) << " " << essential(2, 2) << std::endl;
        output << std::endl;

        output.close();
    }

    void writeTrinsic(std::string folder, ProjectorCalibrateTrinsics &projectorCalibrateTrinics)
    {
        io::createFolder(folder);
        Intrinsic &cameraIntrinsic = projectorCalibrateTrinics.cameraIntrinsic;
        Intrinsic &projectorIntrinsic = projectorCalibrateTrinics.projectorIntrinsic;
        Extrinsic extrinsic = projectorCalibrateTrinics.extrinsic;
        std::string filename = folder + "\\" + "cameraIntrinsic.txt";
        writeIntrinsic(filename, cameraIntrinsic);
        filename = folder + "\\" + "projectorIntrinsic.txt";
        writeIntrinsic(filename, projectorIntrinsic);
        filename = folder + "\\" + "extrinsic.txt";
        writeExtrinsic(filename, extrinsic);
    }

    void writeTrinsic(std::string folder, BiocularCalibrateTrinsics &biocularCalibrateTrinsics)
    {
        io::createFolder(folder);
        Intrinsic &leftIntrinsic = biocularCalibrateTrinsics.leftIntrinsic;
        Intrinsic &rightIntrinsic = biocularCalibrateTrinsics.rightIntrinsic;
        Extrinsic &extrinsic = biocularCalibrateTrinsics.extrinsic;
        std::string filename = folder + "\\" + "leftIntrinsic.txt";
        writeIntrinsic(filename, leftIntrinsic);
        filename = folder + "\\" + "rightIntrinsic.txt";
        writeIntrinsic(filename, rightIntrinsic);
        filename = folder + "\\" + "extrinsic.txt";
        writeExtrinsic(filename, extrinsic);
    }

    void readIntrinsic(std::string filename, Intrinsic &intrinsic)
    {
        std::ifstream input(filename);
        Mat matrix = Mat::zeros(3, 3, CV_32FC1);
        for (int i = 0; i < 3; i++)
        {
            std::string line;
            std::getline(input, line);
            std::stringstream lineStream(line);
            lineStream >> matrix.at<float>(i, 0) >> matrix.at<float>(i, 1) >> matrix.at<float>(i, 2);
        }
        intrinsic.matrix = matrix;
        intrinsic.fx = intrinsic.matrix.at<float>(0, 0);
        intrinsic.fy = intrinsic.matrix.at<float>(1, 1);
        intrinsic.tx = intrinsic.matrix.at<float>(0, 2);
        intrinsic.ty = intrinsic.matrix.at<float>(1, 2);

        std::string line;
        std::getline(input, line);
        std::stringstream lineStream(line);
        for (int i = 0; i < 5; i++)
        {
            float value;
            lineStream >> value;
            intrinsic.kc.push_back(value);
        }
    }

    void readExtrinsic(std::string filename, Extrinsic &extrinsic)
    {
        std::ifstream input(filename);
        Mat matrix = Mat::zeros(4, 4, CV_32FC1);
        std::string line;
        for (int i = 0; i < 4; i++)
        {
            std::getline(input, line);
            std::stringstream lineStream(line);
            lineStream >> matrix.at<float>(i, 0) >> matrix.at<float>(i, 1) >> matrix.at<float>(i, 2) >> matrix.at<float>(i, 3);
        }
        extrinsic.matrix = matrix;
        Mat rotation = Mat::zeros(3, 3, CV_32FC1);
        matrix(Range(0, 3), Range(0, 3)).copyTo(rotation(Range(0, 3), Range(0, 3)));
        extrinsic.rotation = rotation;
        extrinsic.translation(0) = matrix.at<float>(0, 3);
        extrinsic.translation(1) = matrix.at<float>(1, 3);
        extrinsic.translation(2) = matrix.at<float>(2, 3);

        std::getline(input, line);
        Mat fundamental = Mat::zeros(3, 3, CV_32FC1);
        for (int i = 0; i < 3; i++)
        {
            std::getline(input, line);
            std::stringstream lineStream(line);
            lineStream >> fundamental.at<float>(i, 0) >> fundamental.at<float>(i, 1) >> fundamental.at<float>(i, 2);
        }
        extrinsic.fundamental = fundamental;

        std::getline(input, line);
        Mat essential = Mat::zeros(3, 3, CV_32FC1);
        for (int i = 0; i < 3; i++)
        {
            std::getline(input, line);
            std::stringstream lineStream(line);
            lineStream >> essential.at<float>(i, 0) >> essential.at<float>(i, 1) >> essential.at<float>(i, 2);
        }
        extrinsic.essential = essential;
    }
    void readTrinsic(std::string folder, ProjectorCalibrateTrinsics &projectorCalibrateTrinics)
    {
        Intrinsic &cameraIntrinsic = projectorCalibrateTrinics.cameraIntrinsic;
        Intrinsic &projectorIntrinsic = projectorCalibrateTrinics.projectorIntrinsic;
        Extrinsic &extrinsic = projectorCalibrateTrinics.extrinsic;
        std::string filename = folder + "\\" + "cameraIntrinsic.txt";
        readIntrinsic(filename, cameraIntrinsic);
        filename = folder + "\\" + "projectorIntrinsic.txt";
        readIntrinsic(filename, projectorIntrinsic);
        filename = folder + "\\" + "extrinsic.txt";
        readExtrinsic(filename, extrinsic);
    }

    void readTrinsic(std::string folder, BiocularCalibrateTrinsics &biocularCalibrateTrinsics)
    {
        Intrinsic &leftIntrinsic = biocularCalibrateTrinsics.leftIntrinsic;
        Intrinsic &rightIntrinsic = biocularCalibrateTrinsics.rightIntrinsic;
        Extrinsic &extrinsic = biocularCalibrateTrinsics.extrinsic;
        std::string filename = folder + "\\" + "leftIntrinsic.txt";
        readIntrinsic(filename, leftIntrinsic);
        filename = folder + "\\" + "rightIntrinsic.txt";
        readIntrinsic(filename, rightIntrinsic);
        filename = folder + "\\" + "extrinsic.txt";
        readExtrinsic(filename, extrinsic);
    }
}